//----------------------------------------------------------------------------
// Filename: 
//----------------------------------------------------------------------------
#include <p18F4520.h>
#include <adc.h>
#include <delays.h>
#include "servo.h"
#include "ieee_robot.h"
#include "motor.h"

#pragma config LVP = OFF

#pragma config OSC = EC //this sets the oscillator to external clock on
//OSC1 pin (pin 13). The PICDEM 2 plus board
//uses a 4 MHz external clock, other configs
//should use different setting

#pragma config PWRT = OFF //adds approximately 65 ms delay on POR
//as defined in Parameter 33, Table 26-10 of PIC18F4520 datasheet

#pragma config WDT = OFF //this enables control of WDT module
//to be performed in software through SWDTEN bit of WDTCON
//register

#pragma config WDTPS = 256 //this gives a 1:256 postscaler
//value for WDT module, a 1:1 postscaler gives nominal 4ms
//period, thus 1:256 gives 4ms*256 = 1024 ms period, meaning
//WDT register needs to be cleared within 1024 ms when SWDTEN is
//set to prevent device from going into reset state

//local main function
void delay_ms(void);
void pri_idle(void);

/*------------------Global Variables for Interrupt Communication------------------*/
volatile unsigned short int servo_position = SERVO_STRAIGHT;
volatile unsigned short int a2d_result = 0;      //variable for storing A2D results
volatile unsigned short int SLOW_MOTOR_FLAG = 0; //use this flag to know when to
										//slow motor
volatile unsigned short int reverse_flag = 0;
volatile unsigned short int millisecond_count = 0;

#if WISKER_DEBOUNCE == 1
     volatile unsigned char portb_int = 0;
#endif
/*---------------------------------------------------------------------------------*/


void main (void) {
     unsigned short int i = 0,j = 0;
/*-------------------------POR/BOR/WDT Reset Detection-----------------------------*/
#if ENABLE_WDT == 1
     if ( test_bit(RCON,POR_BIT) == 0 ) { //POR occurred (active low)
          //reset in software to enable detection of other reset causes
          RCON = set_bit(RCON,POR_BIT); 
          //enable WDT through SWDTEN bit to force WDT to reset device
          WDTCON = 0x01;
          //wait for a WDT reset to start user code
          while(1) { }
     } else if ( test_bit(RCON,WDT_BIT) == 0 ) { //WDT reset occured
          //disable WDT to prevent further resets
          WDTCON = 0x00;
     }
#endif
/*-----------------------END POR/BOR/WDT Reset Detection---------------------------*/

/*-----------------------Configure all Ports and Interrupt Priority----------------*/
     //init PORT A
     PORTA = 0;
     TRISA = 0xFB; //port a all inputs, except RA2

     //init port B
     PORTB = 0;
     //configure AN8-AN12 (RB0-RB4) as digital I/O lines, AN0-AN7 as analog lines
     ADCON1 = 0x07;        //enable PORTB digital inputs <4:0>
     //RB0,RB3 outputs, the reset are digital inputs
     TRISB = 0x76;
     
     //init PORTC
     PORTC = 0;
     //RC6 - output for TX line, RC3 output for servo, RC2 output for CCP1
     //RC1 & RC0 output for Direction bits to motor controller
     TRISC = 0xB0;

     //init PORTD
     PORTD = 0;
     //RC5 output (must be done for PWM), rest input
     TRISD = 0xDF;

     //init PORTE
     PORTE = 0;

	RCONbits.IPEN = 1;            //enable interrupt priority levels
/*---------------------------------------------------------------------------------*/

#if CHARGE_UP == 1
/*-------------------------ADC SETUP AND CONFIGURATION-----------------------------*/
     //Initialize ADC so that clock is fosc/4, result stored in least significant
     //bits of regester, acuisition time set to 12 TAD, AN0 is input channel, 
     //interrupts are on, refrences points are VDD and VSS, and AN<0:3> are 
     //analog signal while rest are digital
      OpenADC( ADC_FOSC_4 & ADC_RIGHT_JUST & ADC_12_TAD, ADC_CH0 & ADC_INT_ON & ADC_REF_VDD_VSS, 13 );      
      IPR1 = IPR1 & (~0x40); //set A2D as low priority interrupts
	 //enable global and low priority interrupts              
      INTCON = INTCON | 0xC0;
      ConvertADC();                //initial ADC 

     /* --- wait until charged up --- */
	while(a2d_result < VCC_THRESH){
		//while(BusyADC());
		//ADCON0bits.GO = 1;
		//wait until chargeup
	}
	
	/*-------------------------SHUT DOWN ADC-------------------------------------------*/
	CloseADC();
     /*---------------------------------------------------------------------------------*/
#endif


/*--------Servo control setup and interrupt configuration--------------------------*/
     INTCON = 0x20;       // enable TMR0 interrupt
     INTCON = INTCON & (~(1 << 7)) & (~(1 << 2)); //disable all interupts and clear 
                                                  //TMR0IF flag bit
     INTCON2 = 0xF4;               //TMR0 high priority,RB low priority
                                   //INTEDG1/2 rising edge, PortB pullups off
#if SERVO == TIMERS
     /*------------------Timer 0 Setup===============================================*/
     write_timer0(TMR0_VALUE);     //write to Timer 0 the value determine by 
                                   //experimentation to provide 50 Hz interrupts
     write_timer1(SERVO_STRAIGHT); //write a default state to Timer 1 for a default
                                   //servo state
     T0CON = 0x88;  //set up timer0 - prescaler 1:1, with 16
                    //bit selected

     /*=============Timer 1 Setup===============================================*/
     IPR1 = IPR1 | 0x01;           //set Timer 1 interrupts high priority
     PIR1 = PIR1 & (~0x01);        //clear TMR1F interrupt flag
     PIE1 = PIE1 & (~0x01);        //disable Timer 1 interrupts
     T1CON = 0x90;  //setup TICON for TMR1 to use Fosc/4 with 1:2 prescaler
                    //with 16 bit rights and read of TMR1H:TMR1:L
#elif SERVO == CONTROLLERS
     init_usart();
#else
     #warning "No initialization for servo, IEEE_Robot_Main.c"
#endif
     /* --- enable global and low priority interrupts --- */              
     enable_int_all();
/* -----------------END SERVO INIT------------------------------------------------- */


/* -----------------Delay Configuration-------------------------------------------- */
     /* TMR3 : use Fosc/4,1:1 prescaler, 16bit read/writes */
     T3CON = 0x84; 
     /* reset tmr3 to millisecond value*/
     tmr3_reset();

     /* --- disable all interrupts --- */
     disable_int_all();

     /*-------------Configure Timer 3 Interrupts-------------*/
     /* --- Disable TMR3 Interrupts, re-enable later ---  */
     PIE2 = PIE2 & 0x02;
     /* --- Clear TMR3IF --- */
     PIR2 = PIR2 & 0xFD;
     /* --- Low Priority Interrupts for Timer 3--- */
     IPR2 = IPR2 & 0xFD; /* TMR3IP = 0 */


     /* --- re-enable all interrups --- */
     enable_int_all();
     /* --- enable Timer 3 --- */
     T3CON = T3CON & (~0x01); 
/*--------------------------------------------------------------------------------*/

#if CHARGE_UP == 0
     for (j = 0; j < 2000;++j) for (i = 0;i < 1000;++i) _asm nop _endasm
     //Delay10KTCYx(120);
#endif


#if DROP_ARRAY == 1
/*-----------------------------Drop Array-----------------------------------------*/
	//while(1){
     	Go_Right_45();		//jerk pin out with servo
     	for (j = 0; j < 50;++j) for (i = 0;i < 1000;++i) _asm nop _endasm
     	Go_Left_15();
     	/*for (j = 0; j < 10;++j)*/ //for (i = 0;i < 1000;++i) _asm nop _endasm
	//}
/*--------------------------------------------------------------------------------*/
#endif


/*-----------------------------PWM INIT-------------------------------------------*/
     //initialize the PWM signal for motor driver IC
     //note  = 50% duty cycle
     init_ccp1();
     set_duty_ccp1(375); //200 = 40% duty cycle, 250 = 67, 300 = 76%,350=83.6%
					//325 is to low for the duty cycle,350 is to low also
     Set_Motor_Stop(); /* dont go forward yet*/
     start_ccp1();     /* but start the PWM module */
     
     //PIE1 = PIE1 & (~0x06); //clear CCP1E and TMR2IE
/*----------------------------END PWM INIT---------------------------------------*/


/*---------------------------SET UP WHISKER AND BUMPER SENSORS-------------------*/
     //init PORT B
     //Inputs:  INT1 (bumperL), INT2 (bumperR), KBI0(front), KBI1(mid), KBI2(rear)
     //Outputs:  RB0, RB3, RB7  (can be used for debuggin)
     
	INTCONbits.RBIF = 0;			//clear all flags
	INTCON3bits.INT1IF = 0;
	INTCON3bits.INT2IF = 0;
         
#if DEBUG == 1
     PORTBbits.RB3 = 1;
     PORTBbits.RB0 = 0;
     
     /*while(1) {
	     PORTB = ~PORTB;
		delay_ms(100);
	}*/
#endif
     

     //INTCON2 = INTCON2 | 0x30;	  	  //bumper edges are rising edge triggered
     //INTCON3 = INTCON3 & 0x3F;        //set bumper priorities to low 
     //INTCON3 = INTCON3 | 0x18;         //enable INT1 and INT2 for bumpers

     enable_wisker_interrupts();          //enable port change interrupts
/* --------------------------------------------------------------------------------------*/
     
     
/* ----------------------- startup find the inter-wall sequence ------------------------ */
     disable_wisker_interrupts();
     /* --- set servo position --- */
     Go_Left_15();
     
     /* --- go forward --- */
     Set_Motor_Forward();
     
     /* --- delay for 2 secs to make it out of start position --- */
     //for (j = 0; j < 100;++j) for (i = 0;i < 1000;++i) _asm nop _endasm
     
     /* --- renable whisker interrupts for normal operation --- */
     /* --- qualification code doesn't need to do this --- */
     enable_wisker_interrupts();
/* ----------------main loop (go to idle mode) ------------------------------------------ */
     while ( 1 ) {
          /* --- if an port B interrupt on change woke us up from idle --- */
          /* --- delay some amount of time for debounce until they are renabled --- */
          //#if WISKER_DEBOUNCE == 1
          //     if (portb_int) {
          //          portb_int = 0;  /* reset flag until next time */
          //          #if QUALIFICATION == 0
          //               delay_ms(1000); /* delay 1 s */
          //               enable_wisker_interrupts();
          //          #endif
          //     }
          //#endif
          
          /* --- if nothing to do idle the processor, interrupts handle the
               rest --- */
          OSCCON = OSCCON & 0xFC; /* --- clear OSCCON<1:0> --- */        
          OSCCON = OSCCON | 0x80; /* --- set IDLEN, OSCCON<7> --- */     
          _asm 
               sleep     
          _endasm   
     }
}


/*-------------------Local Functions----------------------------------------------*/
void delay_ms(void) {
     unsigned short int x = 0;
     /* reset Timer 3 counter */
     tmr3_reset();
     
     /* enable Timer 3 interrupts */
     //PIE2 = PIE2 | 0x02;
	/* enable timer 3 */
	T3CON = T3CON | 0x01;
	
	while (x < 65535) {
	     	x = TMR3L;
	          x = (TMR3H << 8) | x;
	}
     
     /* disable timer 3 */
     T3CON = T3CON & (~0x01);
}

void pri_idle(void) {
     OSCCONbits.IDLEN = 1;
     OSCCONbits.SCS1 = 0;
     OSCCONbits.SCS0 = 0;

     _asm
          sleep
     _endasm
}

